#include "CRecognition.h"
#include "cmath.h"

CRecognition::CRecognition()
{
        memset(learned,0,sizeof(unsigned char)*3);
	learned[0] = 0;
	learned[1] = 0;
        learned[2] = 0;
	tolerance = 30;

        memset(colorArray,0,64*64*64);
}
        
CRecognition::~CRecognition()
{
}

 
void CRecognition::increaseTolerance()
{
	tolerance+=5;
	if (tolerance > 400) tolerance = 400;
	fprintf(stdout,"Tolerance: %i\n",tolerance);
}

void CRecognition::decreaseTolerance()
{
	tolerance-=5;
	if (tolerance < 0) tolerance = 0;
	fprintf(stdout,"Tolerance: %i\n",tolerance);
}

void CRecognition::learnPixel(unsigned char* a)
{
    // Learn RGB color
	unsigned char u[3];
        for (int i =0;i<3;i++) learned[i] = a[i];
    // Learn in HSV
	rgbToHsv(learned[0],learned[1],learned[2],&learnedHue,&learnedSaturation,&learnedValue);
    // Create color array
        memset(colorArray,0,64*64*64);
        for(int r = 0; r < 256; r+=4)
        {
            u[0] = r;
            for(int g = 0; g < 256; g += 4)
            {
                u[1] = g;
                for(int b = 0; b < 256; b += 4)
                {
                    u[2] = b;
                    int i = ((r/4)*64 + g/4)*64 + b/4;
                    colorArray[i] = evaluatePixelHsv(u);
                }
            }
        }
//	fprintf(stdout,"Learned RGB: %i %i %i, HSV: %i %i %i\n",learned[0],learned[1],learned[2],learnedHue,learnedSaturation,learnedValue);
}

int CRecognition::evaluatePixelHsv(unsigned char* a)
{
	float vxy = 0;
	unsigned int h;
	unsigned char s,v;
	rgbToHsv(a[0],a[1],a[2],&h,&s,&v);
	vxy = vxy + pow((int)h-(int)learnedHue,2);
	vxy = vxy + pow((int)s-(int)learnedSaturation,2)/4;
	vxy = vxy + pow((int)v-(int)learnedValue,2)/16;
        vxy = sqrt(vxy);
	if (vxy > tolerance)
            return 0;
        else
            return 1;
}

int CRecognition::evaluatePixelGray(unsigned char* a)
{
    return 0;
}

int CRecognition::evaluatePixel(unsigned char* a)
{       
    return colorArray[((a[0]/4)*64 + a[1]/4)*64 + a[2]/4];
}

int CRecognition::findPath(CRawImage* im,int *width,int* center)
{       
    int len = im->width*im->height;
    char buffer[len];
    int stack[len];
    int stackPosition = 0;
    // Initial position
    int position = (im->height-10)*im->width + im->width/2;
    int expand[4] = {im->width,-im->width,1,-1};

    memset(buffer,0,len);

    // Learn pixel at initial position
    learnPixel(&im->data[position*3]);

    for(int i=0; i<len; i++)
    {
        buffer[i] = -evaluatePixel(&im->data[i*3]);
    }

    // Trim border
    int pos = (im->height-1)*im->width;
    for(int i=0;i<im->width;i++)
    {
        buffer[i] = 0;
        buffer[pos+i] = 0;
    }
    for(int i = 0;i<im->height;i++)
    {
        buffer[im->width*i] = 0;
        buffer[im->width*i+im->width-1] = 0;
    }

    // Searching (Depth First Search)
    stack[stackPosition++] = position;
    buffer[position] = 1;
    // While stack isn't empty
    while(stackPosition > 0)
    {
        // Take last pixel out stack
        position = stack[--stackPosition];
        // Expand and evalute
        for(int i = 0; i < 4; i++)
        {
            pos = position + expand[i];
            // Positive pixel
            if(buffer[pos] < 0)
            {
                // Insert pixel in stack
                stack[stackPosition++] = pos;
                buffer[pos] = 1;
            }
        }
    }

    for(int y = 0; y < im->height; y++)
    {
        width[y] = 0;
        center[y] = 0;
        int yconst = y*im->width;
        for(int x = 0; x < im->width; x++)
        {
            if(buffer[yconst + x] == 1)
            {
                if(center[y] == 0) center[y] = x;
                else width[y] = x;
            }
        }
        if(width[y] != 0)
        {
         //   center[y] = (int)(center[y]/width[y]);
            int MA = 5;
            if(y >= MA-1)
            {
                for(int yi = 1; yi < MA; yi++)
                {
                    width[y] += width[y-yi];
                    center[y] += center[y-yi];
                }
                width[y] = (int)(width[y]/MA);
                center[y] = (int)(center[y]/MA);
            }
        }
        else
        {
            center[y] = 0;
            width[y] = 0;
        }
        buffer[yconst + center[y]] = 2;
        buffer[yconst + width[y]] = 2;
 //       buffer[yconst - (int)(width[y]/2)] = 2;
    }

    // Coloring
    for(int i=0;i<len;i++)
    {
        switch ((int)buffer[i])
        {
            case 1:
                im->data[i*3] = 0;
                im->data[i*3+1] = 255;
                im->data[i*3+2] = 0;
                break;
            case 2:
                im->data[i*3] = 0;
                im->data[i*3+1] = 0;
                im->data[i*3+2] = 255;
                break;
            default:
                break;
        }
    }
    
    return 1;
}

void CRecognition::rgbToHsv(unsigned char r, unsigned char g, unsigned char b, unsigned int *hue, unsigned char *saturation, unsigned char *value)
{
    float min, max, delta;
    float h,s,v;

    h=s=v=0;
    *saturation = (unsigned char) s;
    *value = (unsigned char) v;
    *hue = (unsigned int) h;

    min = min(r, min(g,b));
    max = max(r, max(g,b));
    v = max;

    delta = max - min;

    if(max != 0)
        s = min(delta*255/max, 255);
    else
    {
        // r=g=b=0
        s = 0;
        h = -1;
        return;
    }

    if(r == max)
        h = (g-b)/delta;
    else if(g == max)
        h = 2 + (b - r)/delta;
    else
        h = 4 + (r - g)/delta;
    h = h*60;
    if(h < 0) h += 360;
    *saturation = (unsigned char) s;
    *value = (unsigned char) v;
    *hue = (unsigned int) h;
}

void CRecognition::rgbToGray(unsigned char r, unsigned char g, unsigned char b, unsigned char *gray)
{
    int gr = 0;
    
    // 30% red, 59% green, 11% blue
    gr = (30*r + 59*g + 11*b)/100;
    *gray = (unsigned char) gr;
}
